#include "Communication/SubBattery.h"
#include "data_struct/robot/BatteryData.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller {
BatteryData DataSet::battery_data;
namespace SubBattery {
static rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr batterySub;

void sub_callback(const sensor_msgs::msg::BatteryState::SharedPtr msg) {
    BatteryData battery_data(msg->voltage, msg->temperature,
                             msg->current, msg->charge, msg->percentage,
                             msg->capacity, msg->present);
    DataSet::battery_data = battery_data;
    DataSet::battery_data.SetTime();
}

void SubBatteryInfo(rclcpp::Node::SharedPtr &node_handle) {
    batterySub = node_handle->create_subscription<sensor_msgs::msg::BatteryState>(
            "battery", rclcpp::SensorDataQoS(), sub_callback);
    std::cout << "SubBattery SubBatteryInfo" << std::endl;
}
} // namespace SubBattery

} // namespace behavior_controller